#ifndef APF_LOCAL_PLANNER_APF_PLANNER_H_
#define APF_LOCAL_PLANNER_APF_PLANNER_H_
#include <Eigen/Core>
#include <vector>
#include <iostream>
#include <stdlib.h>
#include <math.h>

#include <costmap_2d/costmap_2d.h>
#include <nav_msgs/Path.h>

#include <tf/transform_listener.h>
#include <ros/ros.h>

#include <base_local_planner/local_planner_limits.h>
#include <base_local_planner/local_planner_util.h>

#include <global_planner/AnyAngleInterface.h>
#include <global_planner/quadratic_calculator.h>

using namespace std;

//double sum(double *p1,int n);v
namespace dwa_local_planner{
    /**
     * @class APFPlanner
     * @brief A class implementing a local planner using the Artificial Potential Fields
     */
enum DirState {
    FRONT,
    LEFT,
    RIGHT,
    BACK
};
struct Region_
{
    std::vector<costmap_2d::MapLocation> vertex;
    float region_angle;
    bool accessable;
};
class APFPlanner
{
    public:
        /**
         * @brief  Constructor for the planner
         * @param name The name of the planner 
         * @param costmap_ros A pointer to the costmap instance the planner should use
         * @param global_frame the frame id of the tf frame to use
         */
        APFPlanner(std::string name,base_local_planner::LocalPlannerUtil *planner_util);

        /**
         * @brief  Destructor for the planner
         */
        ~APFPlanner()
        {
            delete p_calc_;
            delete anyangle_apf_planner_;
        }

        void setObstacle(std::vector<std::vector <double> > obs)
        {
            obstacles = obs;
        }
      
        void apfInit(double start_point[3], double goal_point[2], std::vector<std::vector<double> >obstacles);
                
        void findBestPath(
            tf::Stamped<tf::Pose> global_pose,
            tf::Stamped<tf::Pose> global_vel,
            tf::Stamped<tf::Pose>& drive_velocities,
            std::vector<geometry_msgs::Point> footprint_spec);

        void setSpeedSmooth( 
            float vx,float vth,   
            tf::Stamped<tf::Pose> global_vel,
            tf::Stamped<tf::Pose>& drive_velocities);  

        bool backRunDetect(
            tf::Stamped<tf::Pose> global_vel,
            tf::Stamped<tf::Pose>& drive_velocities) ;

        bool parking(
            tf::Stamped<tf::Pose> global_pose,
            tf::Stamped<tf::Pose> global_vel,
            tf::Stamped<tf::Pose>& drive_velocities,
            std::vector<geometry_msgs::Point> footprint_spec);

        double start_point[3];
        double goal_point[2];
        double current_vel[3];
        
    private:
        double computePotential();
        std::vector<double> computeAngle();
        std::vector<double> computeAttraction(std::vector<double> att_angle);
        std::vector<double> computeRepulsion(std::vector<double> angle);
        void setSpeedSmooth(float vx, float vth);

        void setSpeed(float vx, float vy, float vth);
        double sum(std::vector<double> p);
        float angle2_pi_pi(float angle);
        double sim_period_,max_vel_x,max_rot_vel,acc_lim_x,acc_lim_theta;

        bool regionSegmention();
        bool getRegionPoint();
        bool caculateOccupiedGrid(std::vector<costmap_2d::MapLocation> &polygon);
        bool judgeDirection(float &angle, DirState dir);        
        void backRun(float angle, tf::Stamped<tf::Pose>& drive_velocities);
        
        void set_position_to_center();
        std::vector<double> find_nearest_obstacle();
        bool avoidAction(tf::Stamped<tf::Pose>& drive_velocities,float angle);
        bool back_run_some_dis(double back_angle, tf::Stamped<tf::Pose>&drive_velocities);
        bool need_avoid(bool expand_flag);

        //parking
        bool iaPathClear();

        double region_radius;
        double clear_radius;
        double back_run_threshold;
        double back_run_speed;
        double trun_speed;
        int region_num;
        double region_angle_turn;
        double region_angle_back;
        double shift_x,shift_y;
        double obstacle_range_short;
        double obstacle_range_long;
        double start_center[2];

        std::vector<Region_> region_front;
        std::vector<Region_> region_back; 
        std::vector<Region_> region_left;
        std::vector<Region_> region_right;

        std::vector<std::vector <double> > obstacles;
        double Attraction_K;
        double Repulsion_K;
        double Obstacles_dis;
        double a;
        int obs_num{0};
        bool avoid_flag{false};

        int apf_num_x ;
        int apf_num_y ;

        tf::Stamped<tf::Pose> drive_velocities_;

        unsigned char *map;
        base_local_planner::LocalPlannerUtil *planner_util_;
        global_planner::PotentialCalculator *p_calc_;
        global_planner::AnyAngleExpansion *anyangle_apf_planner_;

    };
}
#endif

